/* LinkMatrices.java
 *
 * Handles computing matrices for the robot, using forward kinematics to find link locations
 */

import Jama.Matrix;

public class LinkMatrices {
    private Matrix t01, t12, t23, t34;
    private Matrix t0toEnd, t0to3, t0to2;
    
    private double rotation1, rotation2;
    private Line link1, link2, link3;

    public LinkMatrices(Line ln1, Line ln2, Line ln3) {
        link1 = ln1; link2 = ln2; link3 = ln3;
        setSlider(0);
        setJ1(0);
        setJ2(0);
        setEnd();
        //recalculate();
    }

    /*
     * Set matrix for sliding joint
     */
    private void setSlider(double d) {
	    double[][] array1 = {{1,0,0,0},
                             {0,1,0,0},
                             {0,0,1,d},
                             {0,0,0,1}};
        t01 = new Matrix(array1);
    }

    /*
     * Set matrix for first rotating joint
     */
    private void setJ1(double theta) {
        double[][] array2 = {{Math.cos(theta),-Math.sin(theta),0,-150},
                             {0,0,1,0},
                             {-Math.sin(theta),-Math.cos(theta),0,0},
                             {0,0,0,1}};
        t12 = new Matrix(array2);
        rotation1 = theta;
    }

    /*
     * Set matrix for second rotating joint
     */
    private void setJ2(double theta) {
        double[][] array3 = {{Math.cos(theta),-Math.sin(theta),0,-100},
                             {Math.sin(theta),Math.cos(theta),0,0},
                             {0,0,1,0},
                             {0,0,0,1}};
        t23 = new Matrix(array3);
        rotation2 = theta;
    }

    /*
     * Set matrix for end effector
     */
    private void setEnd() {
        double[][] array4 = {{1,0,0,-75},
                             {0,1,0,0},
                             {0,0,1,0},
                             {0,0,0,1}};
        t34 = new Matrix(array4);
    }

    /*
     * Calculates the matrices and updates the coordinates.
     */
    private void recalculate() {
        t0to2 = t01.times(t12);
        t0to3 = t0to2.times(t23);
        t0toEnd = t0to3.times(t34);
        t0to2.print(5, 3);
        t0to3.print(5, 3);
        t0toEnd.print(5, 3);

        // TODO: update link{1,2,3} with the correct coords
        // NOTE: Problem with link2. some notes...
        //       Error is probably in the calculations here or b/c we set up the
        //       coordinate system incorrectly.
        //       For some reason, the coordinates (stored in coords) are in the following
        //       format: {{Y}, {Z}, {X}, {1}}. See the console output.
        //       Messing w/ link2 messes up link3 as well
        double[][] coords = {{0}, {link1.getStartY()}, {link1.getStartX()}, {1}};
        // link1
        Matrix tmp = new Matrix(coords);
        Matrix tmp2 = t01.times(tmp);
        link1.setStartX((int)tmp2.get(2, 0));
        link1.setEndX((int)tmp2.get(2, 0));
        t01.print(5, 3);
        tmp2.print(5, 3);

        // link2
        coords[1][0] = 0;
        coords[0][0] = link1.getStartY();
        coords[2][0] = link1.getStartX();
        tmp = new Matrix(coords);
        tmp2 = t0to3.times(tmp);
        link2.setStartX(link1.getEndX());
        link2.setStartY(link1.getEndY());
        link2.setEndX((int)tmp2.get(1, 0));
        link2.setEndY((int)tmp2.get(0, 0));
        tmp2.print(5, 3);

        // link3
        /*coords[0][0] = 0;
        coords[1][0] = link2.getEndY();
        coords[2][0] = link2.getEndX();
        tmp = new Matrix(coords);*/
        tmp2 = t0toEnd.times(tmp);
        link3.setStartX(link2.getEndX());
        link3.setStartY(link2.getEndY());
        link3.setEndX((int)tmp2.get(1, 0));
        link3.setEndY((int)tmp2.get(0, 0));
        tmp2.print(5, 3);
    }

    /*
     * Slide left (negative) or right (positive) by d
     */
    public void slide(double d) {
        // TODO: check bounds first
        //setSlider(link1.getStartX() + d);
        setSlider(d);
        recalculate();
        setSlider(0);
    }

    /*
     * Rotate link 2 by theta (radians)
     */
    public void rot2(double theta) {
        //setJ1(theta);
        setJ1(rotation1 + theta);
        recalculate();
        //setJ1(0);
    }

    /*
     * Rotate link 3 by theta (radians)
     */
    public void rot3(double theta) {
        //setJ2(theta);
        setJ2(rotation2 + theta);
        recalculate();
        //setJ2(0);
    }

    public Line getLink1() {
        return link1;
    }

    public Line getLink2() {
        return link2;
    }

    public Line getLink3() {
        return link3;
    }
}
